#ifndef _SERVO_YUFENG_H__
#define _SERVO_YUFENG_H__

#include <motor/motor.h>
#include <encoder/encoder_factory.h>

#include <std_msgs/msg/int8.hpp>
#include <std_msgs/msg/int16.hpp>
#include <std_msgs/msg/int32.hpp>
#include <std_msgs/msg/u_int8.hpp>
#include <std_msgs/msg/u_int16.hpp>
#include <std_msgs/msg/u_int32.hpp>


namespace ros_canopen_driver {

struct PDO_CACHE_YUFENG
{ 
    std::atomic<uint8_t>     drive_error{0};
    std::atomic<uint8_t>     steer_error{0};
    std::atomic<uint8_t>     pump_error{0};
    std::atomic<int32_t>     actual_speed{0};
    std::atomic<uint8_t>     battery{0};
    std::atomic<uint8_t>     signals{0};
    std::atomic<int16_t>     steer_angle{0};
    std::atomic<double>      timestamp{0.0};
};

class ServoYufengCurtis : public Motor
{
public:
    ServoYufengCurtis(std::shared_ptr<rclcpp::Node> node, const std::string name);
    ~ServoYufengCurtis();

    int Motor_Init();

    bool Motor_Enable(bool onFlag);

    bool Motor_InitParam();

    bool Motor_HomeMachine();

    bool Motor_InitVmode();

    bool Motor_InitPmode();

    bool Motor_SwitchMode(int8_t mode);

 
    void Motor_SetAcc(uint32_t acc);

    void Motor_SetDec(uint32_t dec);

    void Motor_SetPmodeVel(uint32_t vel);

    void Motor_SetTargetVelRad(double vel);

    void Motor_SetTargetVelEnc(int vel);

    void Motor_SetTargetPosRad(double pos);

    void Motor_SetTargetPosEnc(int pos);


    MotorState Motor_GetState();

    double Motor_GetCurrentVel();

    double Motor_GetCurrentPos();

    double Motor_GetCurrent();

    uint16_t Motor_GetStatusWord();

    uint16_t Motor_GetErrCode();

    int8_t Motor_GetMode();


    void Motor_Brake(bool on);

    void Motor_Shutdown();

    bool Motor_Halt();

    bool Motor_Recover();

    double Motor_GetTimestamp() { return pc_.timestamp;}

    std::string Motor_Err2userErr(const uint16_t& motorErr);

private:
    SERVOMOTOR_PORT m_servo_motor;
    int preTargetEnc;
    std::atomic<MotorState> motorState;
    struct ForkLiftParam fork_param;
    std::shared_ptr<encoder::Encoder> encoder;

    struct PDO_CACHE_YUFENG pc_;

    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::UInt8>> set_fork_lift_down_speed_pub;
    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::UInt8>> set_fork_lift_up_speed_pub;
    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::UInt8>> set_wheel_acc_pub;
    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::UInt8>> set_wheel_dec_pub;
    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::Int16>> set_target_steer_angle_pub;
    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::Int16>> set_wheel_speed_pub;
    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::UInt8>> set_fork_lift_enable_pub;

    rclcpp::Subscription<std_msgs::msg::UInt8>::SharedPtr drive_error_sub;
    rclcpp::Subscription<std_msgs::msg::UInt8>::SharedPtr steer_error_sub;
    rclcpp::Subscription<std_msgs::msg::UInt8>::SharedPtr pump_error_sub;
    rclcpp::Subscription<std_msgs::msg::UInt8>::SharedPtr signals_sub;
    rclcpp::Subscription<std_msgs::msg::Int16>::SharedPtr actual_speed_sub;
    rclcpp::Subscription<std_msgs::msg::UInt8>::SharedPtr battery_sub;
    rclcpp::Subscription<std_msgs::msg::Int16>::SharedPtr steer_angle_sub;

    void drive_error_sub_callback(const std_msgs::msg::UInt8::ConstSharedPtr& msg);    
    void steer_error_sub_callback(const std_msgs::msg::UInt8::ConstSharedPtr& msg);   
    void pump_error_sub_callback(const std_msgs::msg::UInt8::ConstSharedPtr& msg);   
    void signals_sub_callback(const std_msgs::msg::UInt8::ConstSharedPtr& msg);   
    void actual_speed_sub_callback(const std_msgs::msg::Int16::ConstSharedPtr& msg);  
    void battery_sub_callback(const std_msgs::msg::UInt8::ConstSharedPtr& msg);       
    void steer_angle_sub_callback(const std_msgs::msg::Int16::ConstSharedPtr& msg);   

    int servo_encoderGet();

    void servo_checkForkStatus();

    bool servo_setForkVel(const int vel);

    void servo_doForkLift(bool up, int vel = 0);

};

}

#endif
